#include"myslam/save_pose.h"
namespace myslam {
SavePose::SavePose(){}

void SavePose::save_pose(SE3d T,double t){

    Twc_BA_ = T;
    Eigen::Quaterniond q = Eigen::Quaterniond(Twc_BA_.rotationMatrix());
    //q.coeffs().transpose();
    // coeffs()前三维为虚部，后一维为实部
    cout << to_string(t)<<" "
         << Twc_BA_.translation()(0,0)<<" "
         << Twc_BA_.translation()(1,0)<<" "
         << Twc_BA_.translation()(2,0)<<" "
         << q.coeffs().transpose()(0,0)<<" "
         << q.coeffs().transpose()(0,1)<<" "
         << q.coeffs().transpose()(0,2)<<" "
         << q.coeffs().transpose()(0,3)<<" "
            ;

}

}
